#include "car_control.h"

void car_ahead(uint8_t mode , int speed_vel , int pos)
{
  if(mode == speed)
  {
    Emm_V5_Vel_Control(1,0,speed_vel,10,1);
    Emm_V5_Vel_Control(2,1,speed_vel,10,1);
    Emm_V5_Vel_Control(3,0,speed_vel,10,1);
    Emm_V5_Vel_Control(4,1,speed_vel,10,1);
    Emm_V5_Synchronous_motion(0);
  }
  else
  {
    Emm_V5_Pos_Control(1, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(2, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(3, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(4, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Synchronous_motion(0);
  }
}

void car_back(uint8_t mode , int speed_vel , int pos)
{
  if(mode == speed)
  {
    Emm_V5_Vel_Control(1,1,speed_vel,10,1);
    Emm_V5_Vel_Control(2,0,speed_vel,10,1);
    Emm_V5_Vel_Control(3,1,speed_vel,10,1);
    Emm_V5_Vel_Control(4,0,speed_vel,10,1);
    Emm_V5_Synchronous_motion(0);
  }
  else 
  {
    Emm_V5_Pos_Control(1, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(2, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(3, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(4, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Synchronous_motion(0);
  }
}

void car_moveleft(uint8_t mode , int speed_vel , int pos)
{
  if(mode == speed)
  {
    Emm_V5_Vel_Control(1,0,speed_vel,10,1);
    Emm_V5_Vel_Control(2,0,speed_vel,10,1);
    Emm_V5_Vel_Control(3,1,speed_vel,10,1);
    Emm_V5_Vel_Control(4,1,speed_vel,10,1);
    Emm_V5_Synchronous_motion(0);
  }
  else
  {
    Emm_V5_Pos_Control(1, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(2, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(3, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(4, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Synchronous_motion(0);
  }
}

void car_moveright(uint8_t mode , int speed_vel , int pos)
{
  if(mode == speed)
  {
    Emm_V5_Vel_Control(1,1,speed_vel,10,1);
    Emm_V5_Vel_Control(2,1,speed_vel,10,1);
    Emm_V5_Vel_Control(3,0,speed_vel,10,1);
    Emm_V5_Vel_Control(4,0,speed_vel,10,1);
    Emm_V5_Synchronous_motion(0);
  }
  else
  {
    Emm_V5_Pos_Control(1, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(2, 1, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(3, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Pos_Control(4, 0, speed_vel, 10, pos, 0, 1); 
    Emm_V5_Synchronous_motion(0);
  }
}void car_stop(void)
{
  Emm_V5_Stop_Now(0,0);
}

void car_turn(int leftfront,int rightfront,int leftrear,int rightrear,uint8_t mode,int pos)
{
  if(mode == speed)
  {
    Emm_V5_Vel_Control(1,0,leftfront,10,1);
    Emm_V5_Vel_Control(2,1,rightfront,10,1);
    Emm_V5_Vel_Control(3,0,leftrear,10,1);
    Emm_V5_Vel_Control(4,1,rightrear,10,1);
    Emm_V5_Synchronous_motion(0);
  }
  else
  {
    Emm_V5_Pos_Control(1, 0, leftfront, 0, pos, 0, 1); 
    Emm_V5_Pos_Control(2, 1, rightfront, 0, pos, 0, 1); 
    Emm_V5_Pos_Control(3, 0, leftrear, 0, pos, 0, 1); 
    Emm_V5_Pos_Control(4, 1, rightrear, 0, pos, 0, 1); 
    Emm_V5_Synchronous_motion(0);
  }
}